﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using BobcatUtility;

namespace BobcatWiiLib
{
    /// <summary>
    /// This class wraps the WiimoteLib.Wiimote class and makes it a bit more
    /// friendly to use.
    /// 
    /// In particular, the Wiimote class calls back with status events at frequent
    /// regular intervals, regardless of whether any data has changed. This class
    /// checks this status information and raises events only when data has changed.
    /// </summary><remarks>
    /// This class performs smoothing on points received from the IR camera by averaging 
    /// the last few values received. The number of values used in the smoothing is
    /// configurable.
    /// 
    /// Note 1: The events raised by this class occur in a background thread.
    /// 
    /// Note 2: The source indexes used for points by this class are not the same 
    ///         as the source indexes that come from the wiimote. This is because 
    ///         the wiimote's points can change index, but we want to raise events
    ///         with more consistent indexes. To do this we track the points to keep
    ///         the indexes consistent.
    /// </remarks>
    public class WiimoteWrapper
    {
        #region Events

        /// <summary>
        /// Data passed to event handlers from this class.
        /// </summary>
        public class IRPointArgs : EventArgs
        {
            public int Source { get; set; }         // Source index 0 - 3
            public double PositionX { get; set; }   // The positions are doubles as they are averaged, smoothed values
            public double PositionY { get; set; }
            public bool Active { get; set; }         // Whether the IR source is on or off
        }

        /// <summary>
        /// Fires when the status of an IR point changes - i.e. when it changes from on to off
        /// or vice-versa.
        /// </summary>
        public event EventHandler<IRPointArgs> IRStatusChangedEvent;

        /// <summary>
        /// Fires when the (smoothed) position of an IR source has changed.
        /// </summary>
        public event EventHandler<IRPointArgs> IRPositionChangedEvent;

        #endregion

        #region Public properties

        public int SmoothingLevel { get; set; }

        #endregion

        #region Public methods

        /// <summary>
        /// Constructor. The wiimote object passed in should already have
        /// had its Connect() and SetReportType() methods called.
        /// </summary>
        public WiimoteWrapper(WiimoteLib.Wiimote wiimote)
        {
            m_Wiimote = wiimote;
            SmoothingLevel = 3;
        }


        /// <summary>
        /// Registers for events from this wiimote.
        /// </summary>
        public void connect()
        {
            // We create entries in our map to hold info about the 4 possible IR points...
            m_listSourceInfos.Add(new IRSourceInfo { iSource = 0 });
            m_listSourceInfos.Add(new IRSourceInfo { iSource = 1 });
            m_listSourceInfos.Add(new IRSourceInfo { iSource = 2 });
            m_listSourceInfos.Add(new IRSourceInfo { iSource = 3 });

            m_Wiimote.WiimoteChanged += onWiimoteDataChanged;
        }


        /// <summary>
        /// Unregisters for events from the wiimote.
        /// </summary>
        public void disconnect()
        {
            m_Wiimote.WiimoteChanged -= onWiimoteDataChanged;
        }


        /// <summary>
        /// Returns the set of active points.
        /// </summary>
        public IList<IRPointArgs> getActivePoints()
        {
            IList<IRPointArgs> results = new List<IRPointArgs>();
            foreach (IRSourceInfo si in m_listSourceInfos)
            {
                if (si.bActive == false) continue;
                results.Add(getArgsFromSourceInfo(si));
            }
            return results;
        }

        #endregion

        #region Private helper functions

        /// <summary>
        /// Called when there is an update to the wiimote status. This is called 
        /// regularly on a background thread, regardless of whether the wiimote data
        /// has actually changed. 
        /// 
        /// We check the data and raise our own events if the data has changed.
        /// </summary>
        private void onWiimoteDataChanged(object sender, WiimoteLib.WiimoteChangedEventArgs args)
        {
            WiimoteLib.WiimoteState wiimoteState = args.WiimoteState;

            // We check the IR sources for changes...
            checkIRSources(wiimoteState);
        }


        /// <summary>
        /// We check the IR sources to see if they have moved or been turned on or off.
        /// </summary><remarks>
        /// Here we check the four sources from the wiimote and try to determine whether
        /// sources have moved or been turned on or off. If changes have occured we fire
        /// events.
        /// </remarks>
        private void checkIRSources(WiimoteLib.WiimoteState wiimoteState)
        {
            // First we see which points from the wiimote map onto which of the points
            // we are managing. (We do this as the indexes from the wiimote cannot be 
            // relied on to stay the same for the same point.)
            IList<MappedWiimoteIRInfo> listMappedIRInfo = getMappedIRInfo(wiimoteState.IRState.IRSensors);

            // We loop through the sources...
            foreach (MappedWiimoteIRInfo irInfo in listMappedIRInfo)
            {
                // We reset the event flags...
                IRSourceInfo si = m_listSourceInfos[irInfo.iSource];
                si.bStateChanged = false;
                si.bPositionChanged = false;

                // For each source we check the position and the state.
                // Note: These functions do not raise events themselves if they detect a change, as we need both 
                //       pieces of information to raise the events.
                checkIRStateForSource(irInfo);
                checkIRPositionForSource(irInfo);

                // We raise the events...
                if (si.bStateChanged == true)
                {
                    Utility.CrossThreadInvoke(IRStatusChangedEvent, this, getArgsFromSourceInfo(si));
                }
                if(si.bPositionChanged == true)
                {
                    Utility.CrossThreadInvoke(IRPositionChangedEvent, this, getArgsFromSourceInfo(si));
                }
            }
        }

        /// <summary>
        /// Maps the IR points from the wiimote camera to our source-indexes. (We do not use the wiimote 
        /// indexes for points as they can change for the same point.)
        /// </summary><returns>
        /// Returns a list of 4 MappedWiimoteIRInfo objects.
        /// </returns>
        /// <remarks>
        /// Here we check the four sources from the wiimote and try to determine which of our
        /// internal source-infos they map to.
        /// 
        /// This is more tricky than it might be as we cannot rely on the indexes of the 
        /// sources from the wiimote remaining consistent. For example, what was source-0 
        /// in one call to this function might become source-1 in the next call. This makes 
        /// it harder for us to see whether changes have occurred.
        /// 
        /// So when we get new data from the wiimote for a point, we first need to try to
        /// work out which of our stored points it maps to. There is no hard-and-fast way
        /// of doing this, and essentially we are making our "best guess". To do this we are
        /// finding which of our active points are the nearest to the wiimotes points, and
        /// assuming that they are really the same point. 
        /// 
        /// Here is the algorithm we are using for this (it's basically fairly simple, but worth
        /// spelling out.)
        /// 
        /// a. We keep two maps:
        ///    i.  The indexes of our active sources to proximity-info for their nearest wiimote source.
        ///    ii. The indexes of the wiimote's active sources to proximity-info for the nearest of our active sources.
        ///    The proximity-info is a small class that just holds the index of the nearest source and the distance
        ///    between the two sources (our source and the wiimote source).
        ///    
        ///    We initialize these maps so that all the 'other' source indexes are set to -1, which indicates
        ///    that we don't know which our our sources map to which wiimote source, and vice versa.
        ///    
        /// b. We loop through our current set of active sources.
        ///    For each source, we find its distance to each of the active wiimote sources and find the
        ///    wiimote source that is nearest to it. 
        ///    
        ///    When we know the nearest wiimote source, we check the wiimote-source -> our-source map to see 
        ///    if this point is the nearest one seen so far. If so, we assign this information to both maps. 
        ///    If we are nearer than a previous point, we need to reset that point in the our-source -> wiimote-source
        ///    map to be -1 to indicate that we haven't found a match for it.
        /// 
        /// c. Once we have looped through all of our points we should know the correct (or at least best-guess)
        ///    mapping of all the points that were active in both the last calculation cycle and this one. We
        ///    can now check if any points have been switched on or off. 
        ///    
        ///    This is now simple, as any points in the our-source -> wiimote-source map that have -1 as
        ///    the wiimote-source are no longer in the wiimote, and must have been turned off. Similarly
        ///    any points in the wiimote map that do not have a corresponding point in our sources must
        ///    be new points.
        /// 
        /// This is probably sounding more complicated than it is. Here's a worked example:
        /// 
        /// Say we start with this data for our active sources:
        /// 0 = 10, 10
        /// 1 = 40, 40
        /// 2 = 50, 50
        /// 
        /// The wiimote has this data:
        /// 0 = 51, 51
        /// 1 = 12, 12
        /// 
        /// a. We initialize the two maps with -1 for all the 'destination' indexes:
        ///    our-source[0] -> wiimote-source[-1], distance-squared=-1
        ///    our-source[1] -> wiimote-source[-1], distance-squared=-1
        ///    our-source[2] -> wiimote-source[-1], distance-squared=-1
        ///    
        ///    wiimote-source[0] -> our-source[-1], distance-squared=-1
        ///    wiimote-source[1] -> our-source[-1], distance-squared=-1
        /// 
        /// b. We loop through each of our points and find the nearest wiimote point:
        ///    0: Nearest wiimote point is 1, with distance-squared = 8. So the maps are:
        ///       our-source[0] -> wiimote-source[1], distance-squared=8
        ///       wiimote-source[1] -> our-source[0], distance-squared=8
        ///    
        ///    1: Nearest wiimote point is 0, with distance-squared = 242. So the maps are:
        ///       our-source[1] -> wiimote-source[0], distance-squared=242
        ///       wiimote-source[0] -> our-source[1], distance-squared=242
        ///       
        ///    2: Nearest wiimote point is 0, with distance-squared = 2. 
        ///       When we look this up in the wiimote map for point 0, we see that the previous best distance
        ///       was 242. So this point is nearer. We reset the data for the previous best point, and set the
        ///       new data.
        ///       our-source[1] -> wiimote-source[-1], distance-squared=-1
        ///       our-source[2] -> wiimote-source[0], distance-squared=1
        ///       wiimote-source[0] -> our-source[2], distance-squared=1
        ///       
        /// c. The complete maps are now:
        ///    our-source[0] -> wiimote-source[1], distance-squared=8
        ///    our-source[1] -> wiimote-source[-1], distance-squared=-1
        ///    our-source[2] -> wiimote-source[0], distance-squared=1
        ///    
        ///    wiimote-source[0] -> our-source[2], distance-squared=1
        ///    wiimote-source[1] -> our-source[0], distance-squared=8
        /// 
        ///    We look through the maps to see if any points still have a -1 as the mapped point.
        ///    our-source[1] does, so it has been turned off.
        /// 
        /// </remarks>
        private IList<MappedWiimoteIRInfo> getMappedIRInfo(WiimoteLib.IRSensor[] irSensors)
        {
            // We create the set of results (with all points initially inactive.)
            IList<MappedWiimoteIRInfo> results = new List<MappedWiimoteIRInfo>();
            results.Add(new MappedWiimoteIRInfo { iSource = 0 });
            results.Add(new MappedWiimoteIRInfo { iSource = 1 });
            results.Add(new MappedWiimoteIRInfo { iSource = 2 });
            results.Add(new MappedWiimoteIRInfo { iSource = 3 });

            // a. We set up the two maps...
            IDictionary<int, ProximityInfo> mapOurSourcesToWiimoteSources = new Dictionary<int, ProximityInfo>();
            foreach (IRSourceInfo si in m_listSourceInfos)
            {
                if (si.bActive == false) continue;
                mapOurSourcesToWiimoteSources.Add(si.iSource, new ProximityInfo { iSource = si.iSource, iDestinationSource = -1, dDistanceSquared = -1.0 });
            }

            IDictionary<int, ProximityInfo> mapWiimoteSourcesToOurSources = new Dictionary<int, ProximityInfo>();
            for(int iWiimoteSource=0; iWiimoteSource<4; ++iWiimoteSource)
            {
                WiimoteLib.IRSensor irs = irSensors[iWiimoteSource];
                if (irs.Found == false) continue;
                mapWiimoteSourcesToOurSources.Add(iWiimoteSource, new ProximityInfo { iSource = iWiimoteSource, iDestinationSource = -1, dDistanceSquared = -1.0 });
            }

            // b. We loop through our active sources, finding the nearest wiimote source for each one...
            foreach (KeyValuePair<int, ProximityInfo> ourInfoPair in mapOurSourcesToWiimoteSources)
            {
                // We find our position for this source...
                int iSource = ourInfoPair.Key;
                IRSourceInfo si = m_listSourceInfos[iSource];
                double dOurX = si.dSmoothedPositionX;
                double dOurY = si.dSmoothedPositionY;

                ProximityInfo ourProximityInfo = ourInfoPair.Value;

                // We look through the points from the camera and find the index of the one nearest
                // to this point...
                foreach (KeyValuePair<int, ProximityInfo> wiiInfoPair in mapWiimoteSourcesToOurSources)
                {
                    // We find the wii's position for this source...
                    int iDestinationSource = wiiInfoPair.Key;
                    WiimoteLib.IRSensor irs = irSensors[iDestinationSource];
                    double dWiiX = irs.RawPosition.X;
                    double dWiiY = irs.RawPosition.Y;

                    // We find the distance-squared, and see if this point is the nearest...
                    double dDeltaX = dWiiX - dOurX;
                    double dDeltaY = dWiiY - dOurY;
                    double dDistanceSquared = dDeltaX * dDeltaX + dDeltaY * dDeltaY;

                    if (ourProximityInfo.iDestinationSource == -1 || dDistanceSquared < ourProximityInfo.dDistanceSquared)
                    {
                        // This point is the nearest so far...
                        ourProximityInfo.iDestinationSource = iDestinationSource;
                        ourProximityInfo.dDistanceSquared = dDistanceSquared;
                    }
                }

                // Did his point map to any of the wiimote points?
                if (ourProximityInfo.iDestinationSource == -1)
                {
                    // The point didn't map to a wiimot point...
                    continue;
                }

                // By now we know which of the wiimote points our point is nearest to. But is it nearer to this
                // wiimote point than the other of our points that we've seen so far?
                ProximityInfo wiiProximityInfo = mapWiimoteSourcesToOurSources[ourProximityInfo.iDestinationSource];
                if (wiiProximityInfo.iDestinationSource == -1)
                {
                    // There is no other point that thinks it is nearest to this wiimot point...
                    wiiProximityInfo.iDestinationSource = iSource;
                    wiiProximityInfo.dDistanceSquared = ourProximityInfo.dDistanceSquared;
                }
                else
                {
                    // There is already another point that has this wiimote point as its closest point.
                    // So are we closer to it, or is the other point?
                    if (ourProximityInfo.dDistanceSquared < wiiProximityInfo.dDistanceSquared)
                    {
                        // We are the nearer point. We need to reset the destination-point for
                        // the source of ours that thought it mapped to this point, as it doesn't
                        // (we do instead)...
                        int iPreviousOurSourceIndex = wiiProximityInfo.iDestinationSource;
                        ProximityInfo previousOurProximityInfo = mapOurSourcesToWiimoteSources[iPreviousOurSourceIndex];
                        previousOurProximityInfo.iDestinationSource = -1;
                        previousOurProximityInfo.dDistanceSquared = -1.0;

                        // And we set our data instead...
                        wiiProximityInfo.iDestinationSource = iSource;
                        wiiProximityInfo.dDistanceSquared = ourProximityInfo.dDistanceSquared;
                    }
                    else
                    {
                        // We are not the nearest point after all, so we reset our proximity info...
                        ourProximityInfo.iDestinationSource = -1;
                        ourProximityInfo.dDistanceSquared = -1.0;
                    }
                }
            }

            // c. We now know the mapping between our previous set of active points and the wiimote's active points,
            // so we can start filling in the results...
            foreach (KeyValuePair<int, ProximityInfo> pair in mapOurSourcesToWiimoteSources)
            {
                // The results all initialize to inactive, so we only need to fill in the sources
                // that are now active...
                ProximityInfo pi = pair.Value;
                if (pi.iDestinationSource == -1) continue;

                // We find the wiimote data for the point that this source maps to...
                WiimoteLib.IRSensor irs = irSensors[pi.iDestinationSource];
                MappedWiimoteIRInfo mappedInfo = results[pi.iSource];
                mappedInfo.bActive = true;
                mappedInfo.iSource = pi.iSource;
                mappedInfo.iX = irs.RawPosition.X;
                mappedInfo.iY = irs.RawPosition.Y;
            }

            // Are there any wiimote sources that did not map to our sources? If so, then they are new ones...
            foreach (KeyValuePair<int, ProximityInfo> pair in mapWiimoteSourcesToOurSources)
            {
                ProximityInfo pi = pair.Value;
                if (pi.iDestinationSource != -1) continue;

                // We've got a non-mapped point, so we need to add it to the results.
                // We need to find the first available source to assign it to...
                int iFirstAvailableSource;
                for (iFirstAvailableSource = 0; iFirstAvailableSource < 4; ++iFirstAvailableSource)
                {
                    if (results[iFirstAvailableSource].bActive == false) break;
                }
                if (iFirstAvailableSource >= 4)
                {
                    throw new Exception("There seems to be more that 4 active sources.");
                }

                // We get the data from the wiimote...
                WiimoteLib.IRSensor irs = irSensors[pi.iSource];

                // And assign it to the source...
                MappedWiimoteIRInfo mappedInfo = results[iFirstAvailableSource];
                mappedInfo.bActive = true;
                mappedInfo.iSource = iFirstAvailableSource;
                mappedInfo.iX = irs.RawPosition.X;
                mappedInfo.iY = irs.RawPosition.Y;
            }

            return results;
        }


        /// <summary>
        /// We check the position information for the source passed in. If it has changed, we fire an event.
        /// </summary>
        private void checkIRPositionForSource(MappedWiimoteIRInfo irInfo)
        {
            // We only update the position info if the source is active...
            if (irInfo.bActive == false)
            {
                return;
            }

            // We find the source info, and update the position history...
            IRSourceInfo sourceInfo = m_listSourceInfos[irInfo.iSource];
            LinkedList<Point> pointHistory = sourceInfo.PointHistory;
            pointHistory.AddLast(new Point(irInfo.iX, irInfo.iY));

            // We only keep history for a number of points equal to the SmoothingLevel...
            if (pointHistory.Count > SmoothingLevel)
            {
                pointHistory.RemoveFirst();
            }

            // We calculate the smoothed position...
            double dSumX = 0.0;
            double dSumY = 0.0;
            foreach (Point p in pointHistory)
            {
                dSumX += p.X;
                dSumY += p.Y;
            }

            int iCount = pointHistory.Count;
            if (iCount == 0)
            {
                throw new Exception("SmoothingLevel must not be zero. (For no smoothing, set the SmoothingLevel to 1.)");
            }
            double dSmoothedX = dSumX / iCount;
            double dSmoothedY = dSumY / iCount;

            // Has the smoothed position changed? If so, we fire an event...
            double dDeltaX = Math.Abs(dSmoothedX - sourceInfo.dSmoothedPositionX);
            double dDeltaY = Math.Abs(dSmoothedY - sourceInfo.dSmoothedPositionY);
            if (dDeltaX < POSITION_TOLERANCE && dDeltaY < POSITION_TOLERANCE)
            {
                // The smoothed position hasn't changed...
                return;
            }

            // The smoothed position has changed. We store the new position
            // and fire an event...
            sourceInfo.dSmoothedPositionX = dSmoothedX;
            sourceInfo.dSmoothedPositionY = dSmoothedY;
            sourceInfo.bPositionChanged = true;
        }


        /// <summary>
        /// We check the source with the index passed in to see if it has changed state
        /// (i.e. from on to off or vixe versa). If it has, we raise an event.
        /// </summary>
        private void checkIRStateForSource(MappedWiimoteIRInfo irInfo)
        {
            // We find out last set of state information...
            IRSourceInfo sourceInfo = m_listSourceInfos[irInfo.iSource];

            // Has the state changed?
            if (irInfo.bActive == sourceInfo.bActive)
            {
                // The state hasn't changed.
                return;
            }

            // The state has changed, so we update our info and raise an event...
            sourceInfo.bActive = irInfo.bActive;
            sourceInfo.bStateChanged = true;

            // If the source has been turned off, we clear the position history (used for smoothing)
            // as we will want to start a new history (maybe at a new position) when the source is 
            // turned back on again...
            sourceInfo.PointHistory.Clear();
        }

        /// <summary>
        /// We create an IRPointArgs object (to pass as an event param) from an IRSource object.
        /// </summary>
        /// <param name="sourceInfo"></param>
        /// <returns></returns>
        private IRPointArgs getArgsFromSourceInfo(IRSourceInfo sourceInfo)
        {
            IRPointArgs args = new IRPointArgs();
            args.Source = sourceInfo.iSource;
            args.Active = sourceInfo.bActive;
            args.PositionX = sourceInfo.dSmoothedPositionX;
            args.PositionY = sourceInfo.dSmoothedPositionY;
            return args;
        }

        #endregion

        #region Private constants

        private const double POSITION_TOLERANCE = 1.0;  // We fire position-change events if positions have moved by more than this amount.

        #endregion

        #region Private data

        // The wiimote we are wrapping...
        private WiimoteLib.Wiimote m_Wiimote;

        /// <summary>
        /// Holds data from one IR source. This includes the current on/off state, the
        /// current smoothed position etc. This class holds data for one of "our" sources, 
        /// i.e. using our indexes (not wiimote indexes) and holding smoothed position 
        /// data rather than raw wiimote camera data.
        /// </summary>
        private class IRSourceInfo
        {
            public int iSource;
            public bool bActive;
            public LinkedList<Point> PointHistory = new LinkedList<Point>();
            public double dSmoothedPositionX;
            public double dSmoothedPositionY;

            // Flags that tell us if we need to raise events...
            public bool bStateChanged;
            public bool bPositionChanged;
        }
        private IList<IRSourceInfo> m_listSourceInfos = new List<IRSourceInfo>(); // The indexes in this list are our indexes, not the indexes from the wiimote.

        /// <summary>
        /// Holds raw IR data from one source from the wiimote camera, but with the source index
        /// mapped to our source indexes. (We do this by tracking the points as the wiimote source
        /// indexes can change for the same point.)
        /// </summary>
        private class MappedWiimoteIRInfo
        {
            public int iSource;
            public bool bActive;
            public int iX;
            public int iY;
        }

        /// <summary>
        /// Holds information about the distance between two points. One point will
        /// be a point from the wiimote camera, the other will be one of our points
        /// (using our indexes). We could be looking at this information either way 
        /// around, so the variable names do not specify which source is ours and 
        /// which is the wiimotes. Instead, that should be clear from the context 
        /// in which the objects are used.
        /// </summary>
        private class ProximityInfo
        {
            public int iSource;
            public int iDestinationSource;
            public double dDistanceSquared;
        }

        #endregion
    }
}
